#include <iostream>
#include <cmath>
#include "Armor.h"
#include "KalmanFiltering.h"
#include <fstream>
#include <GenICam/System.h>
#include "camera/StreamRetrieve.h"
#include "pnp/PNPSolver.h"


double getDistance(const cv::Point2f &c1, const cv::Point2f &c2) {
    return pow(pow(c1.x - c2.x, 2) + pow(c1.y - c2.y, 2), 0.5);
}

int main() {

    std::vector<double> inputZ_X,inputZ_Y;
    std::vector<double> Hat_X,Hat_XV,Hat_Y,Hat_YV;

    CSystem& systemObj = CSystem::getInstance();

    TVector<ICameraPtr> vCameraPtrList;
    //发现设备
    bool isDiscoverySuccess = systemObj.discovery(vCameraPtrList);
    if (!isDiscoverySuccess)
    {
        printf("discovery device fail.\n");
        return 0;
    }
    if (vCameraPtrList.size() == 0)
    {
        printf("no camera device find.\n");
        return 0;
    }
    //连接设备
    ICameraPtr cameraSptr = vCameraPtrList[0];
    bool isConnect = cameraSptr->connect();
    if (!isConnect)
    {
        printf("connect camera fail.\n");
        return 0;
    }
    //创建流对象
    IStreamSourcePtr streamPtr = systemObj.createStreamSource(cameraSptr);
    if (streamPtr.get() == nullptr)
    {
        printf("create stream obj  fail.\n");
        return 0;
    }
    //开始采集图像
    bool isStartGrabbingSuccess = streamPtr->startGrabbing();
    if (!isStartGrabbingSuccess)
    {
        printf("StartGrabbing  fail.\n");
    }

    //拉流线程
    Dahua::Memory::TSharedPtr<StreamRetrieve> streamThreadSptr(new StreamRetrieve(streamPtr));
    if (NULL == streamThreadSptr.get())
    {
        printf("create thread obj fail.\n");
        return 0;
    }

    streamThreadSptr->start();


    cv::Mat frame, img, hsv, mask;
//    cv::VideoCapture video;
//    video.open("../c.avi");
    namedWindow("video", cv::WINDOW_AUTOSIZE);

    PNPSolver pnpSolver;

    KalmanFiltering kf;

    Eigen::MatrixXd p0(4, 4);
    p0 << 1.0, 0.0, 0.0, 0.0,
            0.0, 100.0, 0.0, 0.0,
            0.0, 0.0, 1.0, 0.0,
            0.0, 0.0, 0.0, 100.0;

    Eigen::MatrixXd inputR(2, 2);
    inputR << 0.1, 0.0,
            0.0, 0.1;

    Eigen::MatrixXd inputH(2, 4);
    inputH << 1.0, 0.0, 0.0, 0.0,
            0.0, 0.0, 1.0, 0.0;

    kf.Init(p0, inputR, inputH);

    double last_timeStamp = 0.0, now_timeStamp;

    while (true) {
        frame = streamThreadSptr->matImage_.clone();
        cv::resize(frame, frame, cv::Size(640, 384));
        cv::Mat channels[3];
        split(frame, channels);
        mask = channels[0] - channels[2];

        threshold(mask, mask, 150, 255, cv::THRESH_BINARY);

        dilate(mask, mask, cv::getStructuringElement(0, cv::Size(3, 3)));
        erode(mask, mask, cv::getStructuringElement(0, cv::Size(3, 3)));
        dilate(mask, mask, cv::getStructuringElement(0, cv::Size(3, 3)));
        erode(mask, mask, cv::getStructuringElement(0, cv::Size(3, 3)));

        dilate(mask, mask, cv::getStructuringElement(0, cv::Size(9, 9)));
//        dilate(mask, mask, cv::getStructuringElement(0, cv::Size(6, 6)));

        imshow("k", mask);
        std::vector<std::vector<cv::Point>> contours;
        std::vector<cv::RotatedRect> choose;
        findContours(mask, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE, cv::Point());
        double area;
        cv::RotatedRect light;

        for (auto &contour: contours) {
            area = contourArea(contour);

            //if (area < 100.0f || area > 900.0f) continue;
            light = minAreaRect(contour);

//            std::cout << "light.angle " << light.angle << std::endl;
//            std::cout << "light.size.width " << light.size.width << std::endl;
//            std::cout << "light.size.height " << light.size.height << std::endl;

            if (((fabs(light.angle) > 70.0f) && (light.size.width > light.size.height) && light.size.width > 10.0f) ||
                ((fabs(light.angle) < 15.0f) && (light.size.height > light.size.width)) && light.size.height > 10.0f
                    ) {
                choose.push_back(light);
            }
        }
        cv::RotatedRect tmp1, tmp2;
        for (int i = 0; i < choose.size(); ++i) {
            tmp1 = choose.at(i);
            for (int j = i + 1; j < choose.size(); ++j) {
                tmp2 = choose.at(j);

                double distance = getDistance(tmp1.center, tmp2.center);
//                std::cout << "distance " << distance << std::endl;
//                std::cout << "rate " << std::max(tmp1.size.height, tmp1.size.width) / std::max(tmp2.size.height, tmp2.size.width) << std::endl;
//                std::cout << "delta y " << tmp1.center.y - tmp2.center.y << std::endl;
//                std::cout << "delta angle " << tmp1.angle - tmp2.angle << std::endl;
//                if (distance <= 36 || distance >= 165)
//                    continue;
//                if (std::max(tmp1.size.height, tmp1.size.width) / std::max(tmp2.size.height, tmp2.size.width) > 1.5 ||
//                    std::max(tmp1.size.height, tmp1.size.width) / std::max(tmp2.size.height, tmp2.size.width) < 0.7)
//                    continue;
//                if (abs(tmp1.center.y - tmp2.center.y) > 10) continue;
//                if (abs(tmp1.angle - tmp2.angle) > 10) continue;

                Armor a1;
                a1.Transform2P(tmp1, tmp2);
                a1.DrawRectangle(frame, cv::Scalar(255, 255, 255));
                pnpSolver.Points2D.clear();
                pnpSolver.Points2D.push_back(a1.pnt[0]);
                pnpSolver.Points2D.push_back(a1.pnt[1]);
                pnpSolver.Points2D.push_back(a1.pnt[2]);
                pnpSolver.Points2D.push_back(a1.pnt[3]);
                pnpSolver.Solve(PNPSolver::METHOD::AP3P);

                //std::cout << dis << std::endl;
                //cv::putText(frame, std::to_string(dis / 1000 - 2.0) + "m", a1.p1, cv::FONT_HERSHEY_SIMPLEX, 1.0f, Salar(0, 255, 0));


                Eigen::MatrixXd inputZ(2, 1);
                inputZ << (double) a1.center.x,
                        (double) a1.center.y;
                std::cout <<"InputZ "<< a1.center << std::endl;
                inputZ_X.push_back((double)a1.center.x);
                inputZ_Y.push_back((double)a1.center.y);
                now_timeStamp = ((double) cv::getTickCount() * 1000.0 / cv::getTickFrequency()) / 1000.0;

                if (!kf.IsInit()) {
                    last_timeStamp = now_timeStamp;
                    Eigen::VectorXd inputX(4, 1);
                    inputX << (double) a1.center.x,
                            2.0,
                            (double) a1.center.y,
                            2.0;
                    Hat_X.push_back((double) a1.center.x);
                    Hat_XV.push_back(2.0);
                    Hat_Y.push_back((double) a1.center.y);
                    Hat_YV.push_back(2.0);
                    kf.InitStateX(inputX);
                }
                cv::circle(frame, a1.center, 12, cv::Scalar(255, 255, 255),3);//绘制检测中心点

                double deltaT = now_timeStamp - last_timeStamp;
                last_timeStamp = now_timeStamp;

                Eigen::MatrixXd inputA(4, 4);
                inputA << 1.0, deltaT, 0.0, 0.0,
                        0.0, 1.0, 0.0, 0.0,
                        0.0, 0.0, 1.0, deltaT,
                        0.0, 0.0, 0.0, 1.0;
                kf.SetA(inputA);

                Eigen::MatrixXd inputQ(4, 4);
                inputQ << 1.0, 0.0, 0.0, 0.0,
                        0.0, 1.0, 0.0, 0.0,
                        0.0, 0.0, 1.0, 0.0,
                        0.0, 0.0, 0.0, 1.0;
//                inputQ << pow(deltaT, 4) / 4.0, 0.0, 0.0, 0.0,
//                0.0, pow(deltaT, 2), 0.0, 0.0,
//                0.0, 0.0, pow(deltaT, 4) / 4.0, 0.0,
//                0.0, 0.0, 0.0, pow(deltaT, 2);
                kf.SetQ(inputQ);

                kf.Predict();
                kf.Correct(inputZ);
                kf.UpdateP();

                //取得预测结果
                Eigen::MatrixXd X_Hat = kf.GetX();
                cv::Point2f center_Hat((float) X_Hat(0), (float) X_Hat(2));
                Hat_X.push_back(X_Hat(0));
                Hat_XV.push_back(X_Hat(1));
                Hat_Y.push_back(X_Hat(2));
                Hat_YV.push_back(X_Hat(3));
                std::cout << "X " << (float) X_Hat(0) << " Y " << (float) X_Hat(2) << std::endl;
                cv::circle(frame, center_Hat, 12, cv::Scalar(0, 255, 0),3);//绘制预测中心点
                cv::rectangle(frame,cv::Point(center_Hat.x-a1.length/2,center_Hat.y-a1.height/2),cv::Point(center_Hat.x+a1.length/2,center_Hat.y+a1.height/2),cv::Scalar(0, 255, 0));
                break;
            }
        }
//        cv::resize(frame,frame,cv::Size(1920,1080));
        imshow("video", frame);
        if (waitKey(1) == 27) {
            //停止采集图像
            streamPtr->stopGrabbing();
            //断开设备连接
            cameraSptr->disConnect();
            streamThreadSptr->stop();

            destroyAllWindows();
            break;
        }
    }
    //=============保存数据
    std::ofstream out("../out.txt");
    std::vector<double> inputZ_XV(inputZ_X.size(),0.0),inputZ_YV(inputZ_Y.size(),0.0);
    if (out.is_open()) {
        auto it = inputZ_X.begin();
        while (it != inputZ_X.end()) {
            out << *it << ",";
            ++it;
        }
        out << "\n";

        it = inputZ_XV.begin();
        while (it != inputZ_XV.end()) {
            out << *it << ",";
            ++it;
        }
        out << "\n";

        it = inputZ_Y.begin();
        while (it != inputZ_Y.end()) {
            out << *it << ",";
            ++it;
        }
        out << "\n";

        it = inputZ_YV.begin();
        while (it != inputZ_YV.end()) {
            out << *it << ",";
            ++it;
        }
        out << "\n";

        it = Hat_X.begin();
        while (it != Hat_X.end()) {
            out << *it << ",";
            ++it;
        }
        out << "\n";

        it = Hat_XV.begin();
        while (it != Hat_XV.end()) {
            out << *it << ",";
            ++it;
        }
        out << "\n";

        it = Hat_Y.begin();
        while (it != Hat_Y.end()) {
            out << *it << ",";
            ++it;
        }
        out << "\n";

        it = Hat_YV.begin();
        while (it != Hat_YV.end()) {
            out << *it << ",";
            ++it;
        }
        out << "\n";

        out.close();
    }
    return 0;
}